// This is the C++ version of objfun.m, which has interface like:
//     function F = objfun(x,feedback,settings)

#include "mex.h"
#include <math>
#include <assert>
#include <stdio>
#include <iostream>
#include "../model.h"

struct MPC_settings
{
    double H_set;
    double H_weight;
    double D_set;
    double D_weight;
    double R2_set;
    double R2_weight;
    double varR2_set;
    double varR2_weight;
    int    horizon;
    int    mode;
    double dt;
    double *step_weight;
};

struct MPC_feedback
{
    double *alphas;
    double *betas;
    double h;
    double D;
};

void objfun(double *J, double W, struct MPC_feedback feedback, struct MPC_settings settings)
{
    model Model;

    Model.setState(feedback);
    
    int i;
    for(i=0;i<settings.P;i++)
    {
        Model.update(
    }
}

void mexFunction(
	int nlhs, mxArray *plhs[],
	int nrhs, const mxArray *prhs[])
{
    if(nrhs!=3)
        mexErrMsgTxt("Three inputs required");
    if(!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0]))
        mexErrMsgTxt("Input 1 must be a vector");
    if(!mxIsStruct(prhs[1]) || mxIsStruct(prhs[2]))
        mexErrMsgTxt("Input 2 and 3 must be a structure");
    
    if(nlhs>1)
        mexErrMsgTxt("Only one output is allowed");

    double *pW;   

    struct MPC_feedback feedback;
    struct MPC_settings settings;
    mxArray *Field;
    double  *pField;
    // Processing feedback: convert a Matlab structure to a C structure
    try
    {
        pField = mxGetPr(mxGetField(prhs[1],0,"alphas"));
        settings.D_weight = &pField;
        pField = mxGetPr(mxGetField(prhs[1],0,"betas"));
        settings.D_weight = &pField;
        pField = mxGetPr(mxGetField(prhs[1],0,"h"));
        settings.D_weight = &pField;
        pField = mxGetPr(mxGetField(prhs[1],0,"SOR"));
        settings.D_weight = &pField;
    }

    // Processing settings: convert a Matlab structure to a C structure
    try
    {
        pField = mxGetPr(mxGetField(prhs[2],0,"Ht_set"));
        settings.H_set = &pField;
        pField = mxGetPr(mxGetField(prhs[2],0,"Fact_H"));
        settings.H_weight = &pField;
        pField = mxGetPr(mxGetField(prhs[2],0,"D_set"));
        settings.D_set = &pField;
        pField = mxGetPr(mxGetField(prhs[2],0,"Fact_D"));
        settings.D_weight = &pField;
        pField = mxGetPr(mxGetField(prhs[2],0,"R2_set"));
        settings.R2_set = &pField;
        pField = mxGetPr(mxGetField(prhs[2],0,"Fact_r2"));
        settings.R2_weight = &pField;
        pField = mxGetPr(mxGetField(prhs[2],0,"Fact_varR2"));
        settings.varR2_weight = &pField;
        pField = mxGetPr(mxGetField(prhs[2],0,"P"));
        settings.horizon = &pField;
        pField = mxGetPr(mxGetField(prhs[2],0,"m"));
        settings.mode = &pField;
        pField = mxGetPr(mxGetField(prhs[2],0,"dt"));
        settings.dt = &pField;
    }
    catch
    {
        cerr << "Error happens when passing settings from Matlab to C\n"
    }
    

    double *pJ;
    plhs[0] = mxCreateDoubleScalar(0);
    J = mxGetPr(plhs[0]);
    
    objfun(pJ, &pW, struct MPC_feedback feedback, struct MPC_settings settings);    
}

